/*
 * pn_lld.cpp
 *
 *  Created on: 09.05.2013
 *      Author: Roma Jam
 */


#include "pn_lld.h"
#include "stdarg.h"

Pn_lvl1_t Pn_lvl1;
// Lvl 1 - Pn cmd's



void Pn_lvl1_t::Init(){
	Pn_Spi.Init();
	Rst.SetupOut(PN_GPIO, PN_RST);
	Rst.Lo();
	Irq.SetupIn(PN_NSS_IRQ_GPIO, PN_IRQ);
	Delay.ms(5);
}

// ================================== PN =======================================
/*
 * Send Cmd to PN and get reply.
 * ALength is count of bytes to send excluding ACmd, i.e. ALength == count of arguments
 */
Pn_PktRslt_t Pn_lvl1_t::Cmd(uint8_t ACmd, uint16_t ALength, ...) {
    PktLen = ALength + 1;
    Buf[0] = ACmd;
    if (ALength != 0) {
        va_list Arg;
        va_start(Arg, ALength);    // Set pointer to last argument
        for(uint32_t i=0; i<ALength; i++) Buf[i+1] = (uint8_t)va_arg(Arg, int);
        va_end(Arg);
    }
//    Uart.Printf("pn cmd: %A\r", Buf, PktLen);
    return WriteAndWaitData(0, 0, Buf, &PktLen);
}

Pn_PktRslt_t Pn_lvl1_t::WriteAndWaitData(uint8_t *PBuf2, uint32_t ALen2, uint8_t *POutBuf, uint32_t *POutLen) {
    Pn_PktRslt_t Response = WriteAndWaitReply(PBuf2, ALen2, POutBuf, POutLen);
    // Now wait for Data for some time
    if(Response == pnPktACK) {    // ACK => proceed with getting reply packet
        uint16_t i=DATA_TIMEOUT;
        while (!Reply_Is_Ready()) {
            Delay.ms(1);
            if(!--i) return pnPktFail;
        }
        // Some reply is ready
        Response = ReadPkt(POutBuf, POutLen);
    } // if ACK
    return Response;
}

Pn_PktRslt_t Pn_lvl1_t::WriteAndWaitReply(uint8_t *PBuf2, uint32_t ALen2, uint8_t *POutBuf, uint32_t *POutLen) {
    WritePkt(PBuf2, ALen2);
    // Now wait for ACK
    uint16_t i=CMD_TIMEOUT;
    while (!Reply_Is_Ready()) {
        Delay.ms(1);
        if(!--i) return pnPktFail;
    }
    // Some reply is ready
    return ReadPkt(PBuf2, &ALen2);
}

// ****************************************************************************
// * Transmits data from two buffers                                          *
// * Always uses extended frame format - for simplicity                       *
// ****************************************************************************
void Pn_lvl1_t::WritePkt(uint8_t *PDataBuf, uint32_t ADataLen) {
//    klPrintf("sd: ");
    uint8_t b;
    uint8_t DCS = PN_TFI_TRANSMIT;
    // Compute length bytes. +1 due to TFI byte
    uint8_t ILngHi = (uint8_t)(0x00FF & ((PktLen+ADataLen+1) >> 8));
    uint8_t ILngLo = (uint8_t)(0x00FF &  (PktLen+ADataLen+1));
    Pn_Spi.Cs.Lo();
    Pn_Spi.WriteReadByte(PN_PRE_DATA_WRITE);   // DW: following operation is writing (p. 42 of user manual)
    Pn_Spi.WriteReadByte(0x00);    // PREAMBLE
    Pn_Spi.WriteReadByte(0x00);    // }
    Pn_Spi.WriteReadByte(0xFF);    // } START CODE: 0x00 0xFF - Start of packet code
    Pn_Spi.WriteReadByte(0xFF);    // }
    Pn_Spi.WriteReadByte(0xFF);    // } Extended frame identifier
    Pn_Spi.WriteReadByte(ILngHi);  // Upper byte of Length
    Pn_Spi.WriteReadByte(ILngLo);  // Lower byte of Length
    Pn_Spi.WriteReadByte(-(ILngHi + ILngLo));     // LCS: Length checksum: (LCS+LEN) = 0
    Pn_Spi.WriteReadByte(PN_TFI_TRANSMIT); // TFI: frame identifier
    // Payload data
    // Choose what data need payload
    if (ADataLen == 0) { // All data is placed to PN532 Buf
#ifdef PN_CMD_PRINT
        klPrintf("wr PnBuf (%u): ", Length);
#endif
    // ======================== Cmd ===========================================
        for (uint8_t i=0; i<PktLen; i++) {
            b = Buf[i];
            Pn_Spi.WriteReadByte(b);
            DCS += b;
#ifdef PN_CMD_PRINT
        klPrintf("%X ", b);
#endif
        }
    }
    else {
#ifdef PN_CMD_PRINT
        klPrintf("wr PnBuf(%u), Data (%u): ", Length, ADataLen);
#endif
    // ======================== Send Data by XfrBlock =========================
        // Send PN532 Cmd Data
        for (uint8_t i=0; i<PktLen; i++) {
            b = Buf[i];
            Pn_Spi.WriteReadByte(b);
            DCS += b;
#ifdef PN_CMD_PRINT
        klPrintf("%X ", b);
#endif
        } // all data PN532 cmd sended
        // Send PC_to_Rdr Data
#ifdef PN_CMD_PRINT
        klPrintf("  ");
#endif
        for(uint16_t i=0; i<ADataLen; i++) {
            b = PDataBuf[i];
            Pn_Spi.WriteReadByte(b);
            DCS += b;
#ifdef PN_CMD_PRINT
        klPrintf("%X ", b);
#endif
        } // all data PC_to_Rdr sended
    } // Send data by Xfr_Block
#ifdef PN_CMD_PRINT
        klPrintf("\r");
#endif
	Pn_Spi.WriteReadByte (-DCS);   // DCS: packet data checksum: (TFI + Cmd+ AData[0] + AData[1] +...+ AData[n] + DCS) = 0
	Pn_Spi.WriteReadByte (0x00);   // POSTAMBLE
	Pn_Spi.Cs.Hi();
}

// ****************************************************************************
// * Recieve data from PN532                                                  *
// * If POutLen is "0" then read all data to Buf                              *
// * If POutLen is not "0" read all data starting from 2 to PDataBuf of RLen  *
// ****************************************************************************
Pn_PktRslt_t Pn_lvl1_t::ReadPkt(uint8_t *PDataBuf, uint32_t *RLen) {
    uint16_t ILen;
    uint8_t b, DCS;
    Pn_Spi.Cs.Lo();
    Pn_Spi.WriteReadByte(PN_PRE_DATA_READ);    // Write a byte indicating that the following operation is reading
    // Skip all zero bytes
    do {
        b = Pn_Spi.WriteReadByte(0);
    } while (b == 0x00);
    // First non-zero byte must be 0xFF; get out if not
    if(b != 0xFF) {
        Pn_Spi.Cs.Hi();
        return (pnPktFail);
    }

    // Recieve next 3 bytes to determine frame type
    Buf[0] = Pn_Spi.WriteReadByte(0);
    Buf[1] = Pn_Spi.WriteReadByte(0);
    Buf[2] = Pn_Spi.WriteReadByte(0);

    uint8_t FCheckSum;
    // Check if ACK: 00_FF_00
    if((Buf[0] == 0x00) and (Buf[1] == 0xFF) and (Buf[2] == 0x00)) {
    	Pn_Spi.Cs.Hi();
        return (pnPktACK);
    }

    // Check if Error Frame: 01_FF_7F...
    else if((Buf[0] == 0x01) and (Buf[1] == 0xFF) and (Buf[2] == 0x7F)) {
        Buf[3] = Pn_Spi.WriteReadByte(0); // must be 0x81
        Buf[4] = Pn_Spi.WriteReadByte(0); // must be 0x00
        Pn_Spi.Cs.Hi();
        // Check if syntax error
        if((Buf[3] != 0x81) or (Buf[4] != 0x00)) return (pnPktFail);
        else return (pnPktErrFrame);
    }

    // Check if Extended Frame: FF_FF...
    else if((Buf[0] == 0xFF) and (Buf[1] == 0xFF)) {
        // R[2] is LENm, already read
        Buf[3] = Pn_Spi.WriteReadByte(0);   // R[3] is LENl
        Buf[4] = Pn_Spi.WriteReadByte(0);   // R[4] is LCS
        Buf[5] = Pn_Spi.WriteReadByte(0);   // TFI
        // Check Length checksum & TFI
        FCheckSum = Buf[2] + Buf[3] + Buf[4];
        if((FCheckSum != 0) or (Buf[5] != PN_TFI_RECIEVE)) {
        	Pn_Spi.Cs.Hi();
            return (pnPktCRC);
        }
        // Calculate Big Length
        ILen = Buf[2];
        ILen <<= 8;
        ILen += Buf[3];
        ILen--; // as LEN includes TFI
    }

    // If normal frame
    else {
        // Buf[0] is LEN, Buf[1] is LCS, Buf[2] is TFI
        // Check Length checksum & TFI
        FCheckSum = Buf[0] + Buf[1];
        if ((FCheckSum != 0) or (Buf[2] != PN_TFI_RECIEVE)) {
        	Pn_Spi.Cs.Hi();
            return (pnPktCRC);
        }
        // CRC ok
        ILen = Buf[0] - 1;  // -1 as LEN includes TFI
    }

    // Receive remainder of packet, discarding received start of packet
    FCheckSum = PN_TFI_RECIEVE;

    // If right command and no error detected
    // ============== Choose the buffer to recieve next data ==================
    *RLen = ILen;
//    klPrintf("Length to PC: %u\r", ILen);
    if (PktLen != 2) {// All data send to PnBuf
#ifdef PN_CMD_PRINT
        klPrintf("rd PnBuf (%u): ", Length);
#endif
        for (uint16_t i=0; i<PktLen; i++) {
            b = Pn_Spi.WriteReadByte(0);
            Buf[i] = b;
            FCheckSum += b;
#ifdef PN_CMD_PRINT
            klPrintf("%X ", b);
#endif
        } // for
    } // all data was send to PnBuf
    else { // Data is needed to send in to DataBuf (to PC)
#ifdef PN_CMD_PRINT
        klPrintf("rd PnBuf (%u): ", Length);
#endif
        for (uint16_t i=0; i<PktLen; i++) {
            b = Pn_Spi.WriteReadByte(0);
            Buf[i] = b;
            FCheckSum += b;
#ifdef PN_CMD_PRINT
            klPrintf("%X ", b);
#endif
        } // for
#ifdef PN_CMD_PRINT
        klPrintf("toPC (%u): ", (ILen-Length));
#endif
        for (uint16_t i=0; i<(ILen-PktLen); i++) {
            b = Pn_Spi.WriteReadByte(0);
            PDataBuf[i] = b;
            FCheckSum += b;
#ifdef PN_CMD_PRINT
            klPrintf("%X ", b);
#endif
        } // for
    } // all data send to PDataBuf
#ifdef PN_CMD_PRINT
    klPrintf("\r");
#endif
    // Recieve DCS
    DCS = Pn_Spi.WriteReadByte(0);
//    klPrintf("DCS: %u\r", DCS);
    FCheckSum += DCS;
    // Check for FCheckSum == 0: TFI+Data[0]+Data[1]+...+DCS must be equal to 0
    if (FCheckSum != 0) {
    	Pn_Spi.Cs.Hi();
        return (pnPktCRC);
    }
    // Recieve postamble
    Pn_Spi.WriteReadByte(0);
    Pn_Spi.Cs.Hi();
    return (pnPktOk);
}
